#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import yaml
from sensor_msgs.msg import JointState

def joint_state_callback(msg):
    # 获取关节位置信息
    joint_positions = msg.position
    
    # 创建YAML格式数据
    yaml_data = {
        'group_state': {
            'name': 'home',
            'group': 'arm',
            'joints': []
        }
    }

    # 填充数据
    for i in range(len(joint_positions) - 1):  # 忽略最后一个关节
        joint_name = "$(arg prefix)joint_" + str(i+1)
        yaml_data['group_state']['joints'].append({
            'name': joint_name,
            'value': joint_positions[i]
        })

    # 打印YAML格式数据
    print('<group_state name="{}" group="{}">'.format(yaml_data['group_state']['name'], yaml_data['group_state']['group']))
    for joint in yaml_data['group_state']['joints']:
        print('\t<joint name="{}" value="{}" />'.format(joint['name'], joint['value']))
    print('</group_state>')

def main():
    rospy.init_node('joint_state_subscriber')
    rospy.Subscriber("/my_gen3/base_feedback/joint_state", JointState, joint_state_callback)
    rospy.spin()

if __name__ == '__main__':
    main()
